#ifndef MEM_TOOLS_H
#define MEM_TOOLS_H

#include "ctest.h"

typedef enum
{
    ERROR   = 0,
    SUCCESS = !ERROR
} ErrorStatus;

static ErrorStatus DMA_memcpy(void *dst, void *src, uint32_t size)
{

}

static ErrorStatus mem_mix_width_write_test(uint32_t base_address, uint32_t size)
{
    uint32_t addr_tmp = base_address;

    while (1) {
        if (addr_tmp < base_address + size) {
            *(volatile uint8_t *)addr_tmp = 0xEF;
            addr_tmp += 1;
        } else {
            break;
        }

        if (addr_tmp < base_address + size - 1) {
            *(volatile uint16_t *)addr_tmp = 0xABCD;
            addr_tmp += 2;
        }
        if (addr_tmp < base_address + size - 3) {
            *(volatile uint32_t *)addr_tmp = 0x12345678;
            addr_tmp += 4;
        }

        if (addr_tmp < base_address + size) {
            *(uint64_t *)addr_tmp = 0xAAAAAAAAAAAAAAAA;
            addr_tmp += 8;
        } else {
            break;
        }
    }

    addr_tmp = base_address;

    while (1) {
        if (addr_tmp < base_address + size) {
            if(*(volatile uint8_t *)addr_tmp != 0xEF) {
                CTEST_ERR("mem_mix_width_write_test ERROR: step 1: p = 0x%x, *p = 0x%x\n", addr_tmp, *(volatile uint8_t *)(addr_tmp));
                break;
            }
            addr_tmp += 1;
        } else {
            break;
        }

        if (addr_tmp < base_address + size - 1) {
            if(*(volatile uint16_t *)addr_tmp != 0xABCD) {
                CTEST_ERR("mem_mix_width_write_test ERROR: step 2: p = 0x%x, *p = 0x%x\n", addr_tmp, *(volatile uint16_t *)(addr_tmp));
            }
            addr_tmp += 2;
        }
        if (addr_tmp < base_address + size - 3) {
            if(*(volatile uint32_t *)addr_tmp != 0x12345678) {
                CTEST_ERR("mem_mix_width_write_test ERROR: step 2: p = 0x%x, *p = 0x%x\n", addr_tmp, *(volatile uint16_t *)(addr_tmp));
                break;
            }
            addr_tmp += 4;
        }

        if (addr_tmp < base_address + size) {
            *(uint64_t *)addr_tmp = 0xAAAAAAAAAAAAAAAA;
            addr_tmp += 8;
        } else {
            break;
        }
    }

    if (addr_tmp == base_address + size) {
        return SUCCESS;
    } else {
        return ERROR;
    }
}

static ErrorStatus mem_align_rw_test(uint32_t base_address, uint32_t size)
{
    uint32_t i;

    for (i = 0; i < size; i += 4) {
        *(volatile uint32_t *)(base_address + i) = 0x5AA55AA5;
    }

    for (i = 0; i < size; i += 4) {
        if (*(volatile uint32_t *)(base_address + i) != 0x5AA55AA5) {
            ASSERT_EQUAL_U(*(volatile uint32_t *)(base_address + i), 0x5AA55AA5);
            CTEST_ERR("mem_align_rw_test ERROR: step 1: p = 0x%x, *p = 0x%x\n", (uint32_t)(base_address + i), *(volatile uint32_t *)(base_address + i));
            return ERROR;
        }
    }

    for (i = 0; i < size; i += 4) {
        *(volatile uint32_t *)(base_address + i) = 0xA55AA55A;
    }

    for (i = 0; i < size; i += 4) {
        if (*(volatile uint32_t *)(base_address + i) != 0xA55AA55A) {
            ASSERT_EQUAL_U(*(volatile uint32_t *)(base_address + i), 0xA55AA55A);
            CTEST_ERR("mem_align_rw_test ERROR: step 2: p = 0x%x, *p = 0x%x\n", (uint32_t)(base_address + i), *(volatile uint32_t *)(base_address + i));
            return ERROR;
        }
    }

   for (i = 0; i < size; i += 8) {
       *(uint64_t *)(base_address + i) = 0x5AA55AA55AA55AA5;
   }

   for (i = 0; i < size; i += 8) {
       if (*(uint64_t *)(base_address + i) != 0x5AA55AA55AA55AA5) {
           ASSERT_EQUAL_U(*(uint64_t *)(base_address + i), 0x5AA55AA55AA55AA5);
           CTEST_ERR("mem_align_rw_test ERROR: step 1: p = 0x%x, *p = 0x%llx\n", (uint32_t)(base_address + i), *(uint64_t *)(base_address + i));
           return ERROR;
       }
   }

   for (i = 0; i < size; i += 8) {
       *(uint64_t *)(base_address + i) = 0xA55AA55AA55AA55A;
   }

   for (i = 0; i < size; i += 8) {
       if (*(uint64_t *)(base_address + i) != 0xA55AA55AA55AA55A) {
           ASSERT_EQUAL_U(*(uint64_t *)(base_address + i), 0xA55AA55AA55AA55A);
           CTEST_ERR("mem_align_rw_test ERROR: step 2: p = 0x%x, *p = 0x%llx\n", (uint32_t)(base_address + i), *(uint64_t *)(base_address + i));
           return ERROR;
       }
   }
    
    return SUCCESS;
}


static ErrorStatus mem_unalign_rw_test(uint32_t base_address, uint32_t size)
{
    uint32_t i;

    for (i = 0; i < size; i++) {
        *(volatile uint8_t *)(base_address + i) = 0x5A;
    }

    for (i = 0; i < size; i++) {
        if (*(volatile uint8_t *)(base_address + i) != 0x5A) {
            ASSERT_EQUAL_U(*(volatile uint8_t *)(base_address + i), 0x5A);
            CTEST_ERR("mem_unalign_rw_test ERROR: step 1: p = 0x%x, *p = 0x%x\n", (uint32_t)(base_address + i), *(volatile uint8_t *)(base_address + i));
            return ERROR;
        }
    }

    for (i = 0; i < size; i++) {
        *(volatile uint8_t *)(base_address + i) = 0xA5;
    }

    for (i = 0; i < size; i++) {
        if (*(volatile uint8_t *)(base_address + i) != 0xA5) {
            ASSERT_EQUAL_U(*(volatile uint8_t *)(base_address + i), 0xA5);
            CTEST_ERR("mem_unalign_rw_test ERROR: step 2: p = 0x%x, *p = 0x%x\n", (uint32_t)(base_address + i), *(volatile uint8_t *)(base_address + i));
            return ERROR;
        }
    }

    return SUCCESS;
}



static ErrorStatus mem_9_step_test(uint32_t base_address, uint32_t size)
{
    uint32_t round;
    uint32_t i;
    uint32_t random;

/* step 1 */
    for (round = 0; round < 2; round++)
    {
/* step 2 */ /* step 6 */
        for (i = 0; i < size; i += 4) {
            *(volatile uint32_t *)(base_address + i) = 0x5A5A5A5A;
        }

        for (i = 0; i < size; i += 4) {
            if (*(volatile uint32_t *)(base_address + i) != 0x5A5A5A5A) {
                ASSERT_EQUAL_U(*(volatile uint32_t *)(base_address + i), 0x5A5A5A5A);
                CTEST_ERR("mem_9_step_test ERROR: step %d: p = 0x%x, *p = 0x%x\n", round * 2 + 2, (uint32_t)(base_address + i), *(volatile uint32_t *)(base_address + i));
                return ERROR;
            }
        }

/* step 3 */ /* step 7 */
        for (i = 0; i < size; i += 4) {
            *(volatile uint32_t *)(base_address + i) = 0xA5A5A5A5;
        }

        for (i = 0; i < size; i += 4) {
            if (*(volatile uint32_t *)(base_address + i) != 0xA5A5A5A5) {
                ASSERT_EQUAL_U(*(volatile uint32_t *)(base_address + i), 0xA5A5A5A5);
                CTEST_ERR("mem_9_step_test ERROR: step %d: p = 0x%x, *p = 0x%x\n", round * 2 + 3, (uint32_t)(base_address + i), *(volatile uint32_t *)(base_address + i));
                return ERROR;
            }

            random = rand();
            *(volatile uint32_t *)(base_address + i) = random;
            if (*(volatile uint32_t *)(base_address + i) != random) {
                ASSERT_EQUAL_U(*(volatile uint32_t *)(base_address + i), random);
                CTEST_ERR("mem_9_step_test ERROR: rand step %d: p = 0x%x, *p = 0x%x\n", round * 2 + 3, (uint32_t)(base_address + i), *(volatile uint32_t *)(base_address + i));
                return ERROR;
            }
        }

/* step 4 */ /* step 8 */
        for (i = size - 4; i != 0; i -= 4) {
            *(volatile uint32_t *)(base_address + i) = 0x5A5A5A5A;
        }

        for (i = size - 4; i != 0; i -= 4) {
            if (*(volatile uint32_t *)(base_address + i) != 0x5A5A5A5A) {
                ASSERT_EQUAL_U(*(volatile uint32_t *)(base_address + i), 0x5A5A5A5A);
                CTEST_ERR("mem_9_step_test ERROR: step %d: p = 0x%x, *p = 0x%x\n", round * 2 + 4, (uint32_t)(base_address + i), *(volatile uint32_t *)(base_address + i));
                return ERROR;
            }
        }

/* step 5 */ /* step 9 */
        for (i = size - 4; i != 0; i -= 4) {
            *(volatile uint32_t *)(base_address + i) = 0xA5A5A5A5;
        }

        for (i = size - 4; i != 0; i -= 4) {
            if (*(volatile uint32_t *)(base_address + i) != 0xA5A5A5A5) {
                ASSERT_EQUAL_U(*(volatile uint32_t *)(base_address + i), 0xA5A5A5A5);
                CTEST_ERR("mem_9_step_test ERROR: step %d: p = 0x%x, *p = 0x%x\n",round * 2 + 5, (uint32_t)(base_address + i), *(volatile uint32_t *)(base_address + i));
                return ERROR;
            }

            random = rand();
            *(volatile uint32_t *)(base_address + i) = random;
            if (*(volatile uint32_t *)(base_address + i) != random) {
                ASSERT_EQUAL_U(*(volatile uint32_t *)(base_address + i), random);
                CTEST_ERR("mem_9_step_test ERROR: rand step %d: p = 0x%x, *p = 0x%x\n",round * 2 + 5, (uint32_t)(base_address + i), *(volatile uint32_t *)(base_address + i));
                return ERROR;
            }
        }
    CTEST_ERR("round %d\n", round);
    }

    return SUCCESS;
}

#define BCKGRND ((uint32_t)0x00000000uL)
#define INV_BCKGRND ((uint32_t)0xFFFFFFFFuL)

static ErrorStatus SRAM_MarchC_Aligned(uint32_t start_addr, uint32_t size)
{
    ErrorStatus result = SUCCESS;
    uint32_t *p = NULL;
    uint32_t end_addr = start_addr + size - 4;

//    uint32_t ra = __return_address(); /* save return address (as it will be destroyed) */

    /* ---------------------------- STEP 1 ----------------------------------- */
    /* Write background with addresses increasing */
    for (p = (uint32_t *)start_addr; p <= (uint32_t *)end_addr; p++) {
        /* Scrambling not important when there's no consecutive verify and write */
        *p = BCKGRND;
    }

    /* ---------------------------- STEP 2 ----------------------------------- */
    /* Verify background and write inverted background with addresses increasing */
    for (p = (uint32_t *)start_addr; p <= (uint32_t *)end_addr; p++) {
        if (*p != BCKGRND) {
            result = ERROR;
            CTEST_ERR("MARCH ERROR: STEP2: p = 0x%x, *p = 0x%x\n", (uint32_t)p, *p);
            return result;
        }

        *p = INV_BCKGRND;
    }

    /* ---------------------------- STEP 3 ----------------------------------- */
    /* Verify inverted background and write background with addresses increasing */

    for (p = (uint32_t *)start_addr; p <= (uint32_t *)end_addr; p++) {
        if (*p != INV_BCKGRND) {
            result = ERROR;
            CTEST_ERR("MARCH ERROR: STEP3: p = 0x%x, *p = 0x%x\n", (uint32_t)p, *p);
            return result;
        }

        *p = BCKGRND;
    }

    /* ---------------------------- STEP 4 ----------------------------------- */
    /* Verify background and write inverted background with addresses decreasing */
    for (p = (uint32_t *)end_addr; p > (uint32_t *)start_addr; p--) {
        if (*p != BCKGRND) {
            result = ERROR;
            CTEST_ERR("MARCH ERROR: STEP4: p = 0x%x, *p = 0x%x\n", (uint32_t)p, *p);
            return result;
        }
        *p = INV_BCKGRND;
    }

    if (p == (uint32_t *)start_addr) {
        if (*p != BCKGRND) {
            result = ERROR;
        }
        *p = INV_BCKGRND;
    }

    /* ---------------------------- STEP 5 ----------------------------------- */
    /* Verify inverted background and write background with addresses decreasing */
    for (p = (uint32_t *)end_addr; p > (uint32_t *)start_addr; p--) {
        if (*p != INV_BCKGRND) {
            result = ERROR;
            CTEST_ERR("MARCH ERROR: STEP5: p = 0x%x, *p = 0x%x\n", (uint32_t)p, *p);
            return result;
        }
        *p = BCKGRND;
    }

    if (p == (uint32_t *)start_addr) {
        if (*p != INV_BCKGRND) {
            result = ERROR;
        }
        *p = BCKGRND;
    }

    /* ---------------------------- STEP 6 ----------------------------------- */
    /* Verify background with addresses increasing */
    for (p = (uint32_t *)start_addr; p <= (uint32_t *)end_addr; p++) {
        if (*p != BCKGRND) {
            result = ERROR; /* No need to take into account scrambling here */
            CTEST_ERR("MARCH ERROR: STEP6: p = 0x%x, *p = 0x%x\n", (uint32_t)p, *p);
            return result;
        }
    }

    /* Restore destroyed return address back into the stack (all the content is destroyed).
       Next line of code supposes the {r4-r5,pc} for Keil(ARMCC 5.06) registers
       only was saved into stack by this test so their restored values are not valid:
       => optiomizations at caller must be switched off as caller cannot relay on r4-r7 values!!!
       The return opcode would be
       POP {r4-r5,pc}
       or
       POP {r4-r7,pc}
       depending on the version of the compiler.
       So it is necessary to skip the registers(r4-r5, or r4-r7), only restore the return address to the
       corrupted stack.*/
//    *((uint32_t *)(__current_sp()) + 2u) = ra;

    return (result);
}

static ErrorStatus SRAM_MarchC_UnalignedStartAddr(uint32_t start_addr, uint32_t size)
{
    ErrorStatus result = SUCCESS;
    uint32_t *p = NULL;
    uint32_t end_addr = start_addr + size - 4;
    start_addr = start_addr +1;

//    uint32_t ra = __return_address(); /* save return address (as it will be destroyed) */

    /* ---------------------------- STEP 1 ----------------------------------- */
    /* Write background with addresses increasing */
    for (p = (uint32_t *)start_addr; p <= (uint32_t *)end_addr; p++) {
        /* Scrambling not important when there's no consecutive verify and write */
        *p = BCKGRND;
    }

    /* ---------------------------- STEP 2 ----------------------------------- */
    /* Verify background and write inverted background with addresses increasing */
    for (p = (uint32_t *)start_addr; p <= (uint32_t *)end_addr; p++) {
        if (*p != BCKGRND) {
            result = ERROR;
            CTEST_ERR("MARCH ERROR: STEP2: p = 0x%x, *p = 0x%x\n", (uint32_t)p, *p);
            return result;
        }

        *p = INV_BCKGRND;
    }

    /* ---------------------------- STEP 3 ----------------------------------- */
    /* Verify inverted background and write background with addresses increasing */

    for (p = (uint32_t *)start_addr; p <= (uint32_t *)end_addr; p++) {
        if (*p != INV_BCKGRND) {
            result = ERROR;
            CTEST_ERR("MARCH ERROR: STEP3: p = 0x%x, *p = 0x%x\n", (uint32_t)p, *p);
            return result;
        }

        *p = BCKGRND;
    }

    /* ---------------------------- STEP 4 ----------------------------------- */
    /* Verify background and write inverted background with addresses decreasing */
    for (p = (uint32_t *)end_addr; p > (uint32_t *)start_addr; p--) {
        if (*p != BCKGRND) {
            result = ERROR;
            CTEST_ERR("MARCH ERROR: STEP4: p = 0x%x, *p = 0x%x\n", (uint32_t)p, *p);
            return result;
        }
        *p = INV_BCKGRND;
    }

    if (p == (uint32_t *)start_addr) {
        if (*p != BCKGRND) {
            result = ERROR;
        }
        *p = INV_BCKGRND;
    }

    /* ---------------------------- STEP 5 ----------------------------------- */
    /* Verify inverted background and write background with addresses decreasing */
    for (p = (uint32_t *)end_addr; p > (uint32_t *)start_addr; p--) {
        if (*p != INV_BCKGRND) {
            result = ERROR;
            CTEST_ERR("MARCH ERROR: STEP5: p = 0x%x, *p = 0x%x\n", (uint32_t)p, *p);
            return result;
        }
        *p = BCKGRND;
    }

    if (p == (uint32_t *)start_addr) {
        if (*p != INV_BCKGRND) {
            result = ERROR;
        }
        *p = BCKGRND;
    }

    /* ---------------------------- STEP 6 ----------------------------------- */
    /* Verify background with addresses increasing */
    for (p = (uint32_t *)start_addr; p <= (uint32_t *)end_addr; p++) {
        if (*p != BCKGRND) {
            result = ERROR; /* No need to take into account scrambling here */
            CTEST_ERR("MARCH ERROR: STEP6: p = 0x%x, *p = 0x%x\n", (uint32_t)p, *p);
            return result;
        }
    }

    /* Restore destroyed return address back into the stack (all the content is destroyed).
       Next line of code supposes the {r4-r5,pc} for Keil(ARMCC 5.06) registers
       only was saved into stack by this test so their restored values are not valid:
       => optiomizations at caller must be switched off as caller cannot relay on r4-r7 values!!!
       The return opcode would be
       POP {r4-r5,pc}
       or
       POP {r4-r7,pc}
       depending on the version of the compiler.
       So it is necessary to skip the registers(r4-r5, or r4-r7), only restore the return address to the
       corrupted stack.*/
//    *((uint32_t *)(__current_sp()) + 2u) = ra;

    return (result);
}


#endif

